within ThreeD_MBS_Dynamics.Examples.Snakeboard;

model Test_RollWheelSet
  parameter SI.Acceleration g=9.81;
  parameter SI.Acceleration[3] Gravity={0,-g,0};
  parameter SI.Mass m_rod=0.5;
  parameter SI.Mass m_disc=0.5;
  parameter SI.Radius r1=0.5;
  parameter SI.Radius r2=0.1;
  parameter SI.Length L=1.5;
  parameter Real coef=0.5;
  parameter SI.AngularVelocity omegaz=0;
  parameter SI.AngularVelocity omega1=0.3;
  parameter SI.Angle phi=0.1;
  parameter SI.Position[3] r0={0,r1,0};
  parameter SI.Velocity[3] v0={-cos(phi),0,sin(phi)};
  parameter SI.Velocity[3] v1=v0 + cross({0,omega1,0}, 0.5*L*{sin(phi),0,cos(phi)});
  parameter SI.Velocity[3] v2=v0 + cross({0,omega1,0}, -0.5*L*{sin(phi),0,cos(phi)});
  parameter SI.Velocity[3] om01={0,omega1,0} - {0,0,v1*{cos(phi),0,-sin(phi)}/r1};
//  SI.Energy Ed;
  SI.Energy E;
  SI.Angle d_phi;
  SI.Position d_x;
  SI.Position d_z;
  SI.Angle phi_pr(start=phi);
  SI.Angle angle;
  SI.Force N;
  Test_DAE Test_DAE1(
    phi0=phi,
    om0=omega1,
    om10=-v1*{cos(phi),0,-sin(phi)}/r1,
    om20=-v2*{cos(phi),0,-sin(phi)}/r1,
    omz0=omegaz,
    v0=v0,
    v10=v1,
    v20=v2,
    r1=r1,
    r2=r2,
    L=L,
    md=m_disc,
    mr=m_rod) annotation (extent=[-20,-26; 20,14]);
  SpringJoint Joint1(
    nA={0,0,1},
    nB={0,0,1},
    rA={0,0,0},
    rB={0,0,0.5*L}) annotation (extent=[-80,34; -40,74]);
//    c = 10,
//    d = 10,
  SpringJoint Joint2(
    nA={0,0,1},
    nB={0,0,1},
    rA={0,0,0},
    rB={0,0,-0.5*L}) annotation (extent=[80,34; 40,74]);
//    c = 10,
//    d = 10,
  Disc Disc1(
    Gravity=Gravity,
    m=m_disc,
    r1=r1,
    q(start={cos(phi/2),0,sin(phi/2),0}),
    omega(start={0,omega1,0} - {0,0,v1*{cos(phi),0,-sin(phi)}/r1}),
    v(start=v1),
    I=m_disc*[0.25*r1^2, 0, 0; 0, 0.25*r1^2, 0; 0, 0, 0.5*r1^2],
    r(start=r0 + 0.5*L*{sin(phi),0,cos(phi)})) 
    annotation (extent=[-80,-26; -40,14], rotation=90);
  Disc Disc2(
    Gravity=Gravity,
    m=m_disc,
    r1=r1,
    omega(start={0,omega1,0} - {0,0,v2*{cos(phi),0,-sin(phi)}/r1}),
    v(start=v2),
    q(start={cos(phi/2),0,sin(phi/2),0}),
    I=m_disc*[0.25*r1^2, 0, 0; 0, 0.25*r1^2, 0; 0, 0, 0.5*r1^2],
    r(start=r0 - 0.5*L*{sin(phi),0,cos(phi)})) 
    annotation (extent=[40,14; 80,-26], rotation=-90);
  Disc_on_Base Disc_on_Plane1(
    ri(start={0,-r1,0}),
    r(start=r0 + 0.5*L*{sin(phi),0,cos(phi)} + {0,-r1,0})) annotation (extent=[-80,-76; -40,-36], rotation=90);
//    R=r1,
  Disc_on_Base Disc_on_Plane2(
    ri(start={0,-r1,0}),
    r(start=r0 - 0.5*L*{sin(phi),0,cos(phi)} + {0,-r1,0})) annotation (extent=[40,-36; 80,-76], rotation=-90);
//    R=r1,
  Rod Rod1(
    Gravity=Gravity,
    m=m_rod,
    r2=r2,
    L=L,
    q(start={cos(phi/2),0,sin(phi/2),0}),
    I=0.25*m_rod*[L^2/3 + r2^2, 0, 0; 0, L^2/3 + r2^2, 0; 0, 0, 2*r2^2],
    v(start=v0),
    omega(start={0,omega1,omegaz}),
    r(start=r0)) annotation (extent=[-20,34; 20,74]);
  annotation (
    Diagram,
    experiment(
      StopTime=50,
      NumberOfIntervals=30000,
      Tolerance=1e-012),
    experimentSetupOutput);
  Base Base1 annotation (extent=[-20,-76; 20,-36]);
  Real[3] i;
  Real[3] j;
  Real[3] k;
  SI.Torque M1;
  SI.Torque M2;
  SI.Torque M3;
  SI.Torque M4;
equation
  i = cross(j, k);
  j = {0,1,0};
  k = Rod1.T*{0,0,1};
  M1 = Rod1.InPortJointA.M*k;
  M2 = Rod1.InPortJointB.M*k;
  M3 = cross(Rod1.InPortJointA.P - Rod1.r, Rod1.InPortJointA.F)*k;
  M4 = cross(Rod1.InPortJointB.P - Rod1.r, Rod1.InPortJointB.F)*k;
//  Ed = Joint1.Ed + Joint2.Ed;
  E = Disc1.K + Disc2.K + Rod1.K;
  angle = Modelica.Math.asin(Rod1.T*{0,0,1}*{0,1,0});
  d_phi = -Test_DAE1.phi + phi_pr;
  d_x = -Test_DAE1.r[1] + Rod1.r[1];
  d_z = -Test_DAE1.r[3] + Rod1.r[3];
  der(phi_pr) = Rod1.OutPort.omega[2];
  N = Disc_on_Plane2.OutPortB.F*Disc_on_Plane2.n - Disc_on_Plane1.OutPortB.F*
    Disc_on_Plane1.n;
  connect(Joint1.OutPortB, Rod1.InPortJointA) 
    annotation (points=[-54,72; -54,84; -6,84; -6,72]);
  connect(Rod1.InPortJointB, Joint2.OutPortB) 
    annotation (points=[6,72; 6,84; 54,84; 54,72]);
  connect(Joint1.InPortB, Rod1.OutPort) 
    annotation (points=[-54,36; -54,26; 0,26; 0,36]);
  connect(Joint2.InPortB, Rod1.OutPort) 
    annotation (points=[54,36; 54,26; 0,26; 0,36]);
  connect(Disc_on_Plane1.InPortA, Base1.OutPort) annotation (points=[-42,-62;
        -30,-62; -30,-86; 0,-86; 0,-74], style(color=3, rgbcolor={0,0,255}));
  connect(Disc_on_Plane2.InPortA, Base1.OutPort) annotation (points=[42,-62; 30,
        -62; 30,-86; 0,-86; 0,-74], style(color=3, rgbcolor={0,0,255}));
  connect(Disc1.OutPort, Disc_on_Plane1.InPortB) annotation (points=[-42,
        -6; -30,-6; -30,-50; -42,-50],
                                   style(color=3, rgbcolor={0,0,255}));
  connect(Disc2.OutPort, Disc_on_Plane2.InPortB) annotation (points=[42,
        -6; 30,-6; 30,-50; 42,-50],
                             style(color=3, rgbcolor={0,0,255}));
  connect(Disc2.InPortRoll, Disc_on_Plane2.OutPortB) annotation (points=[78,-12;
        90,-12; 90,-50; 78,-50], style(color=3, rgbcolor={0,0,255}));
  connect(Disc1.InPortRoll, Disc_on_Plane1.OutPortB) annotation (points=[-78,
        -12; -90,-12; -90,-50; -78,-50], style(color=3, rgbcolor={0,0,255}));
  connect(Joint2.InPortA, Disc2.OutPort) annotation (points=[66,36; 66,22;
        30,22; 30,-6; 42,-6],
                           style(color=3, rgbcolor={0,0,255}));
  connect(Joint1.InPortA, Disc1.OutPort) annotation (points=[-66,36; -66,
        22; -30,22; -30,-6; -42,-6],
                                 style(color=3, rgbcolor={0,0,255}));
  connect(Joint2.OutPortA, Disc2.InPortJoint) annotation (points=[66,72;
        66,84; 90,84; 90,-8.88178e-016; 78,-8.88178e-016],
                                                    style(color=3, rgbcolor={0,
          0,255}));
  connect(Joint1.OutPortA, Disc1.InPortJoint) annotation (points=[-66,72;
        -66,84; -90,84; -90,8.88178e-016; -78,8.88178e-016],
                                                         style(color=3,
        rgbcolor={0,0,255}));
end Test_RollWheelSet;
